//
//  PIDControllerTests.m
//  Podcopter
//
//  Created by Aaron Thompson on 4/5/10.
//  Copyright 2010 Vanderbilt University. All rights reserved.
//

#import "PIDControllerTests.h"
#import "PIDController.h"

@implementation PIDControllerTests

- (void)testProportionalControlWithInertiaAndLag
{
	const double controllerAngle = 5.0;
	double currentAngle = 0.0;
	double currentAngularVelocity = 0.0;
	const double dt = 1.0 / 15.0;
	const double maxOutput = 10.0;
	const double minOutput = -10.0;
	const double outputGain = 1.0;
	const double mass = 1.0;
	
	// Set up value history for lagging
	const int lagCount = 2;
	int curLagIndex = 0;
	double outputValues[lagCount];
	for (int i = 0; i < lagCount; i++) {
		outputValues[i] = 0.0;
	}
	
	int numTestSamples = 500;
	double t = 0.0;
	for (int i = 0; i < numTestSamples; i++)
	{
		double curOutput = (controllerAngle - currentAngle) * outputGain;
		curOutput = fmax(fmin(curOutput, maxOutput), minOutput);
		
		int outputIndex = (curLagIndex + 1) % lagCount;
		double output = outputValues[outputIndex];
		
		// Save the current output into the output history round robin style
		curLagIndex = (curLagIndex + 1) % lagCount;
		outputValues[curLagIndex] = curOutput;
		
		NSLog(@"%.3f	%.4f", t, currentAngle);
		
		// Update the current angle but hold the controller angle constant
		double angularAcceleration = output / mass;
		currentAngularVelocity += angularAcceleration * dt;
		currentAngle += currentAngularVelocity * dt;
		
		t += dt;
	}
}

- (void)testController
{
	PIDController *pid = [[PIDController alloc] init];
	double controllerAngle = 10.0;
	double currentAngle = 0.0;
	double dt = 1.0 / 60.0;
	
	int numTestSamples = 500;
	double t = 0.0;
	for (int i = 0; i < numTestSamples; i++)
	{
		double output = [pid updateOutputWithControllerAngle:controllerAngle currentAngle:currentAngle deltaTime:dt];
		NSLog(@"Output at t %.3f	%f", t, output);
		
		// Update the current angle but hold the controller angle constant
		currentAngle += output * dt;
		
		t += dt;
	}
}

- (void)testControllerWithInertia
{
	PIDController *pid = [[PIDController alloc] init];
	double controllerAngle = 10.0;
	double currentAngle = 0.0;
	double dt = 1.0 / 60.0;
	double maxOutput = 10.0;
	double mass = 2.0;
	
	int numTestSamples = 500;
	double t = 0.0;
	for (int i = 0; i < numTestSamples; i++)
	{
		double output = [pid updateOutputWithControllerAngle:controllerAngle currentAngle:currentAngle deltaTime:dt];
		output = fmin(output, maxOutput);
		NSLog(@"Output at t %.3f	%f", t, output);
		
		// Update the current angle but hold the controller angle constant
		currentAngle += output * dt / mass;
		
		t += dt;
	}
}

- (void)testControllerWithInertiaAndLag
{
	PIDController *pid = [[PIDController alloc] init];
	double controllerAngle = 10.0;
	double currentAngle = 0.0;
	double dt = 1.0 / 60.0;
	double maxOutput = 10.0;
	double mass = 2.0;
	
	// Set up value history for lagging
	const int lagCount = 5;
	int curLagIndex = 0;
	double outputValues[lagCount];
	for (int i = 0; i < lagCount; i++) {
		outputValues[i] = 0.0;
	}
	
	int numTestSamples = 500;
	double t = 0.0;
	for (int i = 0; i < numTestSamples; i++)
	{
		double curOutput = [pid updateOutputWithControllerAngle:controllerAngle currentAngle:currentAngle deltaTime:dt];
		curOutput = fmin(curOutput, maxOutput);
		
		double output = outputValues[curLagIndex];
		
		// Save the current output into the output history round robin style
		curLagIndex = (curLagIndex + 1) % lagCount;
		outputValues[curLagIndex] = curOutput;
		
		NSLog(@"%.3f	%f", t, output);
		
		// Update the current angle but hold the controller angle constant
		currentAngle += output * dt / mass;
		
		t += dt;
	}
}

@end
